package fem2.element.crack_sda;

import inf.jlinalg.ArrayVector;
import inf.jlinalg.IMatrix;
import inf.jlinalg.IVector;
import inf.math.LinAlg;

import java.util.ArrayList;

import math2.LinAlgUtilities;
import math2.MathUtilities;
import fem2.MaterialModel;
import fem2.MeshPart;
import fem2.Node;
import fem2.NotImplementedError;
import fem2.enu.MeshPartType;
import fem2.enu.StressType;
import fem2.material.cohesive_law.CohesiveLaw;

/**
 * Constant jump element using symmetric formulation with enforcing
 * orthogonal-like condition
 * 
 * @author hbui
 * 
 */
public class SOSConstantJumpElement extends ConstantJumpSDAElement {

	private static final int[][] SEPARATION_MODE_TRI3 = new int[][] { { 0 }, { 1 }, { 2 } };
	private static final int[][] SEPARATION_MODE_QUAD4 = new int[][] { { 0 }, { 1 }, { 2 }, { 3 },
			{ 0, 1 }, { 0, 3 } };
	private static final int[][] SEPARATION_MODE_TRI6 = new int[][] { { 0 }, { 1 }, { 2 },
			{ 0, 3 }, { 0, 5 }, { 1, 3 }, { 1, 4 }, { 2, 4 }, { 2, 5 }, { 0, 3, 5 }, { 1, 3, 4 },
			{ 2, 4, 5 } };

	protected int[] posNodes;
	protected boolean enforce_orthogonal_condition = true;

	public SOSConstantJumpElement(MeshPart mp, MaterialModel mm, CohesiveLaw cohesiveLaw) {
		super(mp, mm, cohesiveLaw);
		setCrackMeshPartType(MeshPartType.LINE2);
		setIntegrationOrderOnCrack(1);
	}

	public SOSConstantJumpElement(MeshPart mp, MaterialModel mm, CohesiveLaw cohesiveLaw,
			StressType stressType) {
		super(mp, mm, cohesiveLaw, stressType);
	}

	private int[][] getSeparationConfiguration(MeshPartType mpType) {
		switch (mpType) {
		case TRI3:
			return SEPARATION_MODE_TRI3;
		case QUAD4:
			return SEPARATION_MODE_QUAD4;
		case TRI6:
			return SEPARATION_MODE_TRI6;
		case QUAD8:
			/*
			 * TODO
			 */
			throw new NotImplementedError();
		default:
			throw new Error("this type of mesh part is not supported");
		}
	}

	@Override
	protected void computeEnhancedStrainOperator(MeshPart mp, IMatrix g, double... xi) {
		int dim = mp.getDimension();
		double coeff = -crackMeasure / elementMeasure;
		if (dim == 2) {
			g.set(0, 0, coeff * normal[0]);
			g.set(1, 1, coeff * normal[1]);
			g.set(2, 0, coeff * normal[1]);
			g.set(2, 1, coeff * normal[0]);
		} else if (dim == 3) {
			g.set(0, 0, coeff * normal[0]);
			g.set(1, 1, coeff * normal[1]);
			g.set(2, 2, coeff * normal[2]);

			g.set(3, 0, coeff * normal[1]);
			g.set(3, 1, coeff * normal[0]);

			g.set(4, 1, coeff * normal[2]);
			g.set(4, 2, coeff * normal[1]);

			g.set(5, 0, coeff * normal[2]);
			g.set(5, 2, coeff * normal[0]);
		}
	}

	@Override
	protected void computeProjectionOperator(MeshPart mp, IMatrix g, double... xi) {
		computeEnhancedStrainOperator(mp, g, xi);
	}

	@Override
	protected boolean checkOnsetCriteria() {
		MeshPart mp = getMeshPart();
		int dim = mp.getDimension();

		double o1 = 0;
		double oxx_max = 0, oyy_max = 0, oxy_max = 0;

		if (dim == 2) {
			int ng = getNumIntegrationPoints();
			/*
			 * seek the Gauss point contains max sigma1
			 */
			for (int i = 0; i < ng; i++) {
				IVector sigma = getStress(i, stressType);
				double oxx = sigma.get(0);
				double oyy = sigma.get(1);
				double oxy = sigma.get(2);
				double t1 = (oxx + oyy) / 2;
				double t2 = Math.sqrt(Math.pow((oxx - oyy) / 2, 2) + Math.pow(oxy, 2));
				if (t1 + t2 > o1) {
					o1 = t1 + t2;
					oxx_max = oxx;
					oyy_max = oyy;
					oxy_max = oxy;
				}
			}

		} else if (dim == 3) {
			/*
			 * TODO
			 */
		}

		if (o1 < getCohesiveLaw().getTensileStrength()) {
			return false;
		} else {
			alpha = new ArrayVector(dim);

			/*
			 * compute sigma1 direction
			 */
			double[] nc = new double[dim];
			if (dim == 2) {
				if (Math.abs(oxy_max) < MathUtilities.tol14) {
					if (oxx_max > oyy_max) {
						nc[0] = 1;
						nc[1] = 0;
					} else {
						nc[0] = 0;
						nc[1] = 1;
					}
				} else {
					double g = Math.sqrt(Math.pow(oxx_max - o1, 2) + Math.pow(oxy_max, 2));
					nc[0] = -oxy_max / g;
					nc[1] = (oxx_max - o1) / g;
				}
			} else if (dim == 3) {
				/*
				 * TODO
				 */
			}

			/*
			 * search the sum of gradient which make smallest angle with crack
			 * normal
			 */
			double cos = 0.0;
			int[][] configurations = getSeparationConfiguration(mp.getType());
			for (int i = 0; i < configurations.length; i++) {
				double[] sum_grad = new double[dim];
				int ng = getNumIntegrationPoints();
				for (int k = 0; k < ng; k++) {
					double[] xi = getIntegrationPoint(k);
					double w = getIntegrationWeight(k);
					double[][] jac = mp.jacXAt(xi);
					double detJ = LinAlgUtilities.determinantNxN(jac);
					double coeff = w * detJ;
					double[][] grad = mp.gGradientsAt(xi);
					for (int j = 0; j < configurations[i].length; j++) {
						for (int l = 0; l < dim; l++) {
							sum_grad[l] += coeff * grad[configurations[i][j]][l];
						}
					}
				}
				double norm_sum_grad = LinAlg.norm2(dim, sum_grad);
				for (int k = 0; k < dim; k++) {
					sum_grad[k] /= norm_sum_grad;
				}
				double tmp = LinAlg.dot(dim, nc, sum_grad);
				if (Math.abs(tmp) > cos) {
					cos = Math.abs(tmp);
					if (enforce_orthogonal_condition) {
						LinAlg.copy(dim, sum_grad, normal);
					} else {
						LinAlg.add(dim, Math.signum(tmp), nc, 0.0, normal);
					}
					/*
					 * compute crack measure
					 */
					crackMeasure = norm_sum_grad;
					/*
					 * store positive nodes
					 */
					posNodes = configurations[i];
				}
			}

			if (dim == 2) {
				tangent1[0] = -normal[1];
				tangent1[1] = normal[0];
			} else if (dim == 3) {
				/*
				 * TODO
				 */
			}

			// Debugger.watch("normal = ", normal);
			// Debugger.watch("tangent = ", tangent1);
			// Debugger.watch("crack measure = ", crackMeasure);
			// Debugger.watch("element measure = ", elementMeasure);

			/*
			 * compute crack boundary
			 */
			crackBoundary = computeCrackBoundaryByScanning();

			return true;
		}
	}

	private ArrayList<double[]> computeCrackBoundaryByScanning() {
		/*
		 * scan all the projection coordinate of all vertex nodes
		 */
		MeshPart mp = getMeshPart();
		Node[] nodes = mp.getNodes();
		double[] s = null;

		if (mp.getType() == MeshPartType.TRI3 || mp.getType() == MeshPartType.TRI6) {
			s = new double[3];
		} else if (mp.getType() == MeshPartType.QUAD4 || mp.getType() == MeshPartType.QUAD8
				|| mp.getType() == MeshPartType.QUAD9) {
			s = new double[4];
		} else {
			throw new Error("this type of mesh part is not supported");
		}

		for (int i = 0; i < s.length; i++) {
			s[i] = project(nodes[i].getPosition());
		}

		/*
		 * sort all the projection coordinate in descending order
		 */
		int[] ids = MathUtilities.sort(s, true);
		double[] sa = MathUtilities.find(s, ids);

		/*
		 * compute the boundary for the specific mesh part type
		 */
		if (mp.getType() == MeshPartType.TRI3 || mp.getType() == MeshPartType.TRI6) {
			return computeCrackBoundaryByScanningForTriangle(crackMeasure, sa,
					nodes[ids[0]].getPosition(), nodes[ids[1]].getPosition(),
					nodes[ids[2]].getPosition());
		} else if (mp.getType() == MeshPartType.QUAD4 || mp.getType() == MeshPartType.QUAD8
				|| mp.getType() == MeshPartType.QUAD9) {
			/*
			 * swap the last 2 index to ensure a valid geomatry
			 */
			if (Math.abs(sa[2] - sa[3]) < MathUtilities.tol6) {
				int tmp = ids[2];
				ids[2] = ids[3];
				ids[3] = tmp;
			}
			return computeCrackBoundaryByScanningForQuadrilateral(crackMeasure, sa,
					nodes[ids[0]].getPosition(), nodes[ids[1]].getPosition(),
					nodes[ids[2]].getPosition(), nodes[ids[3]].getPosition());
		} else {
			throw new Error("this type of mesh part is not supported");
		}
	}

	/*************************************************************
	 * Utilities function
	 *************************************************************/

	private double project(double[] X) {
		int dim = getMeshPart().getDimension();
		double p = LinAlg.norm2(dim, normal);
		return LinAlg.dot(dim, X, normal) / Math.pow(p, 2);
	}

	private double[] computeIntersection(double s, double[] X1, double[] X2) {
		int dim = getMeshPart().getDimension();
		double d = -s * Math.pow(LinAlg.norm2(dim, normal), 2);
		if (dim == 2) {
			return MathUtilities.computeIntersection(normal[0], normal[1], d, X1[0], X1[1], X2[0]
					- X1[0], X2[1] - X1[1]);
		} else if (dim == 3) {
			return MathUtilities.computeIntersection(normal[0], normal[1], normal[2], d, X1[0],
					X1[1], X1[2], X2[0] - X1[0], X2[1] - X1[1], X2[2] - X1[2]);
		} else {
			throw new Error("invalid dimension");
		}
	}

	/*************************************************************
	 * Triangle
	 *************************************************************/

	private double computeLengthInTriangleDomain1(double s, double[] X1, double[] X2, double[] X3) {
		int dim = getMeshPart().getDimension();
		double[] c1 = computeIntersection(s, X1, X2);
		double[] c2 = computeIntersection(s, X1, X3);
		return LinAlg.diffNorm2(dim, c1, c2);
	}

	private double computeLengthInTriangleDomain2(double s, double[] X1, double[] X2, double[] X3) {
		int dim = getMeshPart().getDimension();
		double[] c1 = computeIntersection(s, X2, X3);
		double[] c2 = computeIntersection(s, X1, X3);
		return LinAlg.diffNorm2(dim, c1, c2);
	}

	private ArrayList<double[]> computeCrackBoundaryByScanningForTriangle(double l, double[] s,
			double[] X1, double[] X2, double[] X3) {
		ArrayList<double[]> bound = new ArrayList<double[]>();
		/*
		 * scan in domain 1
		 */
		if (s[0] != s[1]) {
			double l1 = computeLengthInTriangleDomain1(s[0], X1, X2, X3);// l1
																			// should
																			// be
																			// 0
			double l2 = computeLengthInTriangleDomain1(s[1], X1, X2, X3);
			assert l1 < l2;
			/*
			 * interpolate projection coordinate based on length
			 */
			double sl = s[0] + (s[1] - s[0]) * (l - l1) / (l2 - l1);
			if (s[1] <= sl && sl <= s[0]) {
				bound.add(computeIntersection(sl, X1, X2));
				bound.add(computeIntersection(sl, X1, X3));
				return bound;
			}
		}
		/*
		 * scan in domain 2
		 */
		if (s[1] != s[2]) {
			double l2 = computeLengthInTriangleDomain2(s[1], X1, X2, X3);
			double l3 = computeLengthInTriangleDomain2(s[2], X1, X2, X3);// l3
																			// should
																			// be
																			// 0
			assert l3 < l2;
			/*
			 * interpolate projection coordinate based on length
			 */
			double sl = s[1] + (s[2] - s[1]) * (l - l2) / (l3 - l2);
			if (s[2] <= sl && sl <= s[1]) {
				bound.add(computeIntersection(sl, X2, X3));
				bound.add(computeIntersection(sl, X1, X3));
				return bound;
			}
		}
		throw new Error("could not find the compatible crack length");
	}

	/*************************************************************
	 * Quadrilateral
	 *************************************************************/

	private double computeLengthInQuadrilateralDomain1(double s, double[] X1, double[] X2,
			double[] X3, double[] X4) {
		int dim = getMeshPart().getDimension();
		double[] c1 = computeIntersection(s, X1, X2);
		double[] c2 = computeIntersection(s, X1, X3);
		return LinAlg.diffNorm2(dim, c1, c2);
	}

	private double computeLengthInQuadrilateralDomain2(double s, double[] X1, double[] X2,
			double[] X3, double[] X4) {
		int dim = getMeshPart().getDimension();
		double[] c1 = computeIntersection(s, X1, X3);
		double[] c2 = computeIntersection(s, X2, X4);
		return LinAlg.diffNorm2(dim, c1, c2);
	}

	private double computeLengthInQuadrilateralDomain3(double s, double[] X1, double[] X2,
			double[] X3, double[] X4) {
		int dim = getMeshPart().getDimension();
		double[] c1 = computeIntersection(s, X2, X4);
		double[] c2 = computeIntersection(s, X3, X4);
		return LinAlg.diffNorm2(dim, c1, c2);
	}

	private ArrayList<double[]> computeCrackBoundaryByScanningForQuadrilateral(double l,
			double[] s, double[] X1, double[] X2, double[] X3, double[] X4) {
		ArrayList<double[]> bound = new ArrayList<double[]>();
		double l1 = 0, l2 = 0, l3 = 0, l4 = 0;
		/*
		 * scan in domain 1
		 */
		if (s[0] != s[1]) {
			l1 = computeLengthInQuadrilateralDomain1(s[0], X1, X2, X3, X4);// l1
																			// should
																			// be
																			// 0
			l2 = computeLengthInQuadrilateralDomain1(s[1], X1, X2, X3, X4);
			assert l1 < l2;
			/*
			 * interpolate projection coordinate based on length
			 */
			double sl = s[0] + (s[1] - s[0]) * (l - l1) / (l2 - l1);
			if (s[1] <= sl && sl <= s[0]) {
				bound.add(computeIntersection(sl, X1, X2));
				bound.add(computeIntersection(sl, X1, X3));
				return bound;
			}
		}

		/*
		 * scan in domain 2
		 */
		if (s[1] != s[2]) {
			l2 = computeLengthInQuadrilateralDomain2(s[1], X1, X2, X3, X4);
			l3 = computeLengthInQuadrilateralDomain2(s[2], X1, X2, X3, X4);
			/*
			 * interpolate projection coordinate based on length
			 */
			double sl = 0;
			if (l2 != l3) {
				sl = s[1] + (s[2] - s[1]) * (l - l2) / (l3 - l2);
			} else {
				if (Math.abs(l - l2) < MathUtilities.tol6) {
					sl = (s[1] + s[2]) / 2;
				}
			}
			if (s[2] <= sl && sl <= s[1]) {
				bound.add(computeIntersection(sl, X2, X4));
				bound.add(computeIntersection(sl, X1, X3));
				return bound;
			}

		}

		/*
		 * scan in domain 3
		 */
		if (s[2] != s[3]) {
			l3 = computeLengthInQuadrilateralDomain3(s[2], X1, X2, X3, X4);
			l4 = computeLengthInQuadrilateralDomain3(s[3], X1, X2, X3, X4); // l4
																			// should
																			// be
																			// 0
			assert l3 > l4;
			/*
			 * interpolate projection coordinate based on length
			 */
			double sl = s[2] + (s[3] - s[2]) * (l - l3) / (l4 - l3);
			if (s[3] <= sl && sl <= s[2]) {
				bound.add(computeIntersection(sl, X2, X4));
				bound.add(computeIntersection(sl, X3, X4));
				return bound;
			}
		}

		/*
		 * if couldn't find the compatible crack length then the middle length
		 * of middle section is used
		 */
		double sl = (s[1] + s[2]) / 2;
		bound.add(computeIntersection(sl, X2, X4));
		bound.add(computeIntersection(sl, X1, X3));
		return bound;

		// Debugger.watch("l1 = ", l1);
		// Debugger.watch("l2 = ", l2);
		// Debugger.watch("l3 = ", l3);
		// Debugger.watch("l4 = ", l4);
		// Debugger.watch("element id = " + getId());
		// Debugger.watch("X1 = ", X1);
		// Debugger.watch("X2 = ", X2);
		// Debugger.watch("X3 = ", X3);
		// Debugger.watch("X4 = ", X4);
		// Debugger.watch("s = ", s);
		// Debugger.watch("normal = ", normal);
		// Debugger.watch("crack length = ", l);
		// throw new Error("could not find the compatible crack length");
	}
}
